error('请不要直接运行，使用运行节代替');

%% 轨迹生成模板




%阶跃
t_ed = 1;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;

step_value = 0;


%斜坡
t_ed = 2;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;

vel_value = pi/4;
t_speed = 1;
pos_st = 0;
slope_traj_value = slope_traj_info(vel_value,t_speed,t_ed,simu_period,pos_st);


%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;

pos_st = 0;
pos_ed = pi/2;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);
%使用此轨迹记得后面使用add_step



%生成速度S轨迹
t_jerk = 1;
t_half_ed = 2.5;
t_half_idle = 1;


simu_period = 1e-4;
simu_step = (t_half_ed+t_half_idle)*2/simu_period;
sample_period = simu_period* 5;
pos_st = 0;
jerk_value = pi/20;
vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_half_ed,t_half_idle,simu_period,pos_st);

%%

clc;
clear;





%参数组初始化
initial_state = zeros(2,3);
is_g_enable = [1;1];
n_factor = 21;
tor_motor = jointMotor(1);

%这里使用的表达不同了，直接以水平为开始位置
pos_st = 0;
pos_ed = pi/2;
t_ed = 3;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;


%阶跃，即位置保持
step_value = pi/2;

%斜坡
vel_value = pi/4;
t_speed = 1;
slope_traj_value = slope_traj_info(vel_value,t_speed,t_ed,simu_period,pos_st);

%生成位置S轨迹
t_acc = 1;
t_ed = 3;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);


%生成速度S轨迹
t_jerk = 0.25;
t_half_ed = 1;
t_half_idle = 0.5;
jerk_value = 4*pi/3;
vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_half_ed,t_half_idle,simu_period,pos_st);



%% 3-20& 3-21 观察关节力矩用,然后跑5-15的代码

clc;
clear;
t_acc = 1;
t_ed = 3;
pos_st = pi/2;
pos_ed = pi;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-a*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);


simu_period = 1e-5;
sample_period = 1e-3;
simu_step = t_ed/simu_period;

pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);


%生成速度S轨迹
t_jerk = 0.25;
t_half_ed = 1;
t_half_idle = 0.5;
jerk_value = 4*pi/3;
vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_half_ed,t_half_idle,simu_period,pos_st);




%% 3-23 用
clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 21;
tor_motor = jointMotor(1);


% 轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;
pos_st = 0.1;
pos_ed = 0.15;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);



% 双关节作业需要
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);
y = 0;
dy = 0;

%% 5-15/5-16          竖直平面内的无电机的关节位置S跟踪-pid控制/位置S的驱动力矩计算
clc;
clear;
t_acc = 1;
t_ed = 3;
pos_st = pi/2;
pos_ed = pi;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-a*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);


simu_period = 1e-5;
sample_period = 1e-3;
simu_step = t_ed/simu_period;

pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);





%% 5-17
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   注意：每更换完一次控制器后（即是否有积分、前馈、集中前馈等），都要来这里跑一次
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
t_acc = 1;
t_ed = 3;
pos_st = pi/2;
pos_ed = pi;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-a*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);


simu_period = 1e-5;
sample_period = 2e-5;
simu_step = t_ed/simu_period;

real_m = 0.5;
real_l = 0.1;


%debug用
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = 2e-4;
%

%阶跃，即位置保持
step_value = pi/2;

%斜坡
vel_value = pi/4;
t_speed = 1;
slope_traj_value = slope_traj_info(vel_value,t_speed,t_ed,simu_period,pos_st);

%生成位置S轨迹
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);

%生成速度S轨迹
t_jerk = 1;
t_half_ed = 2.5;
t_half_idle = 1;


simu_period = 1e-4;
vel_simu_step = (t_half_ed+t_half_idle)*2/simu_period;
sample_period = simu_period* 5;
jerk_value = pi/20;
vel_s_traj_value = vel_s_traj_info(jerk_value,t_jerk,t_half_ed,t_half_idle,simu_period,pos_st);



%ref_data = [0 4 1e4;0 8 1e4;1 0.6124 1e5;1 1.26248 1e5];
%working mode, control voltage, steps
%  速度         uc=4
%  速度         uc=8
%  力矩         uc=0.61

err = 0.95; %杆长和质量的偏差
%要求：竖直工况，n=50/10，杆长、质量有5%负偏差，分别研究速度和力矩模式，4种轨迹
%现在是因为可能需要一次性获得多组数据，所以使用n_group来规定需要几组数据
n_group = 2;
n_factor = [10;50];
initial_state =[pi/2 0 0 ; pi/2 0 0 ];
is_g_enable = eye(2);

tor_motor = jointMotor(1);

% simu_period = 1e-5;
% simu_step = 1e4;
% sample_period = 1e-3;
%注意数据存储方式，是组号- 时间 - 数据类型（加速度速度或者位置）
res = zeros(n_group,length(1:simu_step),3);
motor_res = zeros(n_group,length(1:simu_step),3);
state_info = zeros(n_group,length(1:simu_step),3);
debug_info = zeros(n_group,length(1:simu_step),4);

size(res);
%
% a_sj = newSingleJoint(initial_state,is_g_enable,n_factor);
% a_sj.update_m_l(err);
a_sj = simpSingleJoint(initial_state,is_g_enable,n_factor);
a_sj.update_m_l(err);



%% 5-18
clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 50;
tor_motor = jointMotor(1);


%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
sample_period = simu_period* 5;
simu_step = t_ed/simu_period;
add_step = 0;
simu_step = 1*simu_step;

pos_st = 0;
pos_ed = pi/4;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);
%使用此轨迹记得后面使用add_step


% 双关节作业需要
initial_state = zeros(2,3);
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);
y = 0;
dy = 0;

%% 6-8

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   注意：每更换完一次控制器后（即是否有积分、前馈、集中前馈等），都要来这里跑一次
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
t_ed = 3;
simu_period = 1e-5;
sample_period = 2e-5;
simu_step = t_ed/simu_period;

real_m = 0.5;
real_l = 0.1;


%debug用
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = 5e-4;
%


% hw 第6章 需要 6-8
%ts = 1, zeta = 1
step_value = 0;
initial_state = [5*pi/180 0 0];
is_g_enable = eye(2);
n_factor = [12;12];
err = 0.1;
a_sj = simpSingleJoint(repmat(initial_state,2,1),is_g_enable,n_factor);
a_sj.update_m_l(1-err);
n_group = 2;
tor_motor = jointMotor(1);

%% 6-9
clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 50;
tor_motor = jointMotor(1);



%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
sample_period = simu_period* 5;
simu_step = t_ed/simu_period;
add_step = 0;
simu_step = 1*simu_step;

pos_st = 0;
pos_ed = pi/4;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);
%使用此轨迹记得后面使用add_step


% 双关节作业需要
initial_state = zeros(2,3);
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);
y = 0;
dy = 0;

%% 6-10

clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 50;
tor_motor = jointMotor(1);

%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
sample_period = simu_period* 5;
simu_step = t_ed/simu_period;
add_step = 0;
simu_step = 1*simu_step;
pos_st = 0.1;
pos_ed = 0.15;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);



% 双关节作业需要
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);
y = 0;
dy = 0;

%% 7-1


clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 50;
tor_motor = jointMotor(1);

%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
sample_period = simu_period* 5;
simu_step = t_ed/simu_period;
add_step = 0;
simu_step = 1*simu_step;
pos_st = 0.1;
pos_ed = 0.15;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);



% 双关节作业需要
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);
y = 0;
dy = 0;



%% 7-2



clc;
clear;

%公共
is_g_enable = [1;1];
n_factor = 50;
tor_motor = jointMotor(1);

%生成位置S轨迹
t_acc = 1;
t_ed = 3;
simu_period = 1e-4;
sample_period = simu_period* 5;
simu_step = t_ed/simu_period;
add_step = 0;
simu_step = 1*simu_step;

pos_st = 0.1;
pos_ed = 0.06;
step_value = pos_ed;

pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);



% 双关节作业需要
initial_state = [pi/3 0 0; -pi*2/3 0 0 ];
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
a_dj.reset_state(initial_state);


%%

% 双关节作业需要
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);


%%

% hw-6_9 需要 & 7-1 & 7-2需要
n_factor = 50;
a_dj = doubleJoint(initial_state,is_g_enable,n_factor);

%%
% 
% % hw 3-23 需要 & hw 6-10需要 &hw 7-1都需要
% pos_st = 0.1;
% pos_ed = 0.15;
% pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
% a_value = fsolve(pos_final,0);
% pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);

%%

% hw 第6章 需要 6-8
%ts = 1, zeta = 1
step_value = 0;
initial_state = [5*pi/180 0 0];
is_g_enable = eye(2);
n_factor = [12;12];
err = 0.1;
a_sj = simpSingleJoint(repmat(initial_state,2,1),is_g_enable,n_factor);
a_sj.update_m_l(1-err);
n_group = 2;


%%
% %hw 7-2 需要
% pos_st = 0.1;
% pos_ed = 0.06;
% step_value = pos_ed;
